#include <iostream>
#include <ode/ode.h>

#include "bunny_geom.h"


#include "Touchable.hh"

#define scaleFactor 0.015625f

dWorldID Touchable::sOdeWorld;
dSpaceID Touchable::sSpace;

Touchable::Touchable() : Renderable("undefined", 0, 0, 0, NULL){
  mUsable = false;
  mLastMatrixIndex = 0;
}

Touchable::Touchable(const std::string& name, double x, double y, double z, const char* md3file, double mass, double velx, double vely, double velz) : Renderable(name, x, y, z, md3file){
  mUsable = false;
  mMass = mass;
  //mOdeMass.mass = mMass;
  mVelx = velx;
  mVely = vely;
  mVelz = velz;
  mLastMatrixIndex = 0;
}

void
Touchable::on_AddWorld(dWorldID world, dSpaceID space){

  mUsable = true;

  sOdeWorld = world;
  sSpace = space;
  mOdeBody = dBodyCreate(sOdeWorld);
  dBodySetPosition (mOdeBody,GetPosX(),GetPosY(),GetPosZ());

  

  // The ODE trimesh stuff ... I'm geting mad with this.
  libmd3_mesh* meshp;
  meshp = GetModel()->meshes;

  tri = (dTriIndex*) malloc (meshp->mesh_header->triangle_count*3*sizeof(dTriIndex));
  triVert = (dVector3*) malloc (meshp->mesh_header->vertex_count*sizeof(dVector3));
  
  std::cout << meshp->mesh_header->triangle_count << std::endl;
  std::cout << meshp->mesh_header->vertex_count << std::endl;

  std::cout << "Faces: ";
  for (int i=0;i < meshp->mesh_header->triangle_count; i++){
    tri[i*3+0]=meshp->triangles[i*3+0];
    tri[i*3+1]=meshp->triangles[i*3+2];
    tri[i*3+2]=meshp->triangles[i*3+1];
    //std::cout << tri[i] << " ";
    //std::cout << tri[i+1] << " ";
    //std::cout << tri[i+2] << " - ";    
  }

  std::cout << " Verts: ";
  for (int i=0;i < meshp->mesh_header->vertex_count; i++){
    triVert[i][0]=(float) meshp->vertices[i*3+0]*scaleFactor; 
    triVert[i][1]=(float) meshp->vertices[i*3+1]*scaleFactor; 
    triVert[i][2]=(float) meshp->vertices[i*3+2]*scaleFactor; 
    //std::cout << "(" << triVert[i][0] << " ";
    //std::cout << triVert[i][1] << " ";
    //std::cout << triVert[i][2] << ") - ";
    
  }
  
    mtriMesh = dGeomTriMeshDataCreate();
  dGeomTriMeshDataBuildSimple(mtriMesh, (dReal*)triVert, meshp->mesh_header->vertex_count,  (const dTriIndex*) tri, \
  			      meshp->mesh_header->triangle_count);

  //dGeomTriMeshDataBuildSingle(mtriMesh, (dReal*) triVert, 3 * sizeof(float), meshp->mesh_header->vertex_count, 
  //  			      (const dTriIndex*) tri, meshp->mesh_header->triangle_count, 3 * sizeof(dTriIndex));

  // dGeomTriMeshDataBuildSingle(mtriMesh, &Vertices[0], 3 * sizeof(float), VertexCount,
  //	       (dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex));


  
  mOdeGeom = dCreateTriMesh(sSpace, mtriMesh, 0, 0, 0);
  dGeomSetData(mOdeGeom, (void*) GetName().c_str());

  //dMassSetZero( &mOdeMass );
  //dMassAdjust( &mOdeMass, mMass ); 
  // DENSITY ...

  printf("\nmass at %f %f %f\n", mOdeMass.c[0], mOdeMass.c[1], mOdeMass.c[2]);
  dMassSetTrimesh( &mOdeMass, mMass, mOdeGeom ); // //////////////////////////////////////////////////////
  //dMassSetSphere(&mOdeMass, mMass, 3); 
  printf("\nmass at %f %f %f\n", mOdeMass.c[0], mOdeMass.c[1], mOdeMass.c[2]);
  
  dGeomSetPosition(mOdeGeom, -mOdeMass.c[0], -mOdeMass.c[1], -mOdeMass.c[2]);
  dMassTranslate( &mOdeMass, -mOdeMass.c[0] ,-mOdeMass.c[1],-mOdeMass.c[2]);
  printf("\nmass_ at %f %f %f\n", mOdeMass.c[0], mOdeMass.c[1], mOdeMass.c[2]);
 

  dGeomSetBody(mOdeGeom, mOdeBody);
  dBodySetLinearVel(mOdeBody, mVelx, mVely, mVelz);
  dBodySetMass(mOdeBody, &mOdeMass);

  std::cout << "Added touchable" << std::endl;
}

Touchable&
Touchable::operator=(const Touchable& t){
  mMass = t.mMass;
  return *this;
}


void
Touchable::SetPos(double x, double y, double z){
  if (mUsable)  {
      Renderable::SetPos(x,y,z);
      dBodySetPosition (mOdeBody,x,y,z); 
    } else {
    std::cout << "Trying to SetPos() of an Entity linked with no world" << std::endl;
  }
}

void
Touchable::SetMass(double mass){
  if (mUsable) {
      mMass = mass;
      mOdeMass.mass = mMass;
      dBodySetMass (mOdeBody, &mOdeMass);  
    } else {
    std::cout << "Trying to SetMass() of an Entity linked with no world" << std::endl;
  }
}

double
Touchable::GetMass(){
  return mMass;
}

void
Touchable::Render(){
  const dReal* _dvPos;
  const dReal* _dvR;
  const dReal* Pos;
  const dReal* Rot;
  GLdouble dvTransMatrix[16];

  _dvPos = dBodyGetPosition(mOdeBody);
  _dvR = dBodyGetRotation(mOdeBody);

  Rot = _dvR;
  Pos = _dvPos;

  dvTransMatrix[0]=  _dvR[0];
  dvTransMatrix[1]=  _dvR[4];
  dvTransMatrix[2]=  _dvR[8];
  dvTransMatrix[3]=  0;
  dvTransMatrix[4]=  _dvR[1];
  dvTransMatrix[5]=  _dvR[5];
  dvTransMatrix[6]=  _dvR[9];
  dvTransMatrix[7]=  0;
  dvTransMatrix[8]=  _dvR[2];
  dvTransMatrix[9]=  _dvR[6];
  dvTransMatrix[10]= _dvR[10];
  dvTransMatrix[11]= 0;
  dvTransMatrix[12]= _dvPos[0];
  dvTransMatrix[13]= _dvPos[1];
  dvTransMatrix[14]= _dvPos[2];
  dvTransMatrix[15]= 1;


  dReal* p_matrix = mMatrixDblBuf + ( mLastMatrixIndex * 16 );

  p_matrix[ 0 ] = Rot[ 0 ];     p_matrix[ 1 ] = Rot[ 1 ];       p_matrix[ 2 ] = Rot[ 2 ];       p_matrix[ 3 ] = 0;
  p_matrix[ 4 ] = Rot[ 4 ];     p_matrix[ 5 ] = Rot[ 5 ];       p_matrix[ 6 ] = Rot[ 6 ];       p_matrix[ 7 ] = 0;
  p_matrix[ 8 ] = Rot[ 8 ];     p_matrix[ 9 ] = Rot[ 9 ];       p_matrix[10 ] = Rot[10 ];       p_matrix[11 ] = 0;
  p_matrix[12 ] = Pos[ 0 ];     p_matrix[13 ] = Pos[ 1 ];       p_matrix[14 ] = Pos[ 2 ];       p_matrix[15 ] = 1;
  

  // Flip to other matrix.
  mLastMatrixIndex = !  mLastMatrixIndex;
  
  dGeomTriMeshSetLastTransform( mOdeGeom, 
				*(dMatrix4*)(  mMatrixDblBuf + mLastMatrixIndex * 16  ) );
				
  

  SetPos(_dvPos[0],_dvPos[1],_dvPos[2]);
  
  glPushMatrix();
  glMultMatrixd(dvTransMatrix);
  //Renderable::Render();
  glPopMatrix();
   
     glPushMatrix();
    dVector4 v1, v2, v3;
  const dReal *realP;
  glColor3f(1.0, 0.0, 0.0);
  //  realP = dGeomGetPosition(mOdeGeom);
  // glTranslatef(realP[0], realP[1], realP[2]);
  glBegin(GL_TRIANGLES);
  for (int i = 0; i < GetModel()->meshes->mesh_header->triangle_count; ++i) {
    dGeomTriMeshGetTriangle(mOdeGeom, i, (dReal(*)[4]) v1, (dReal(*)[4]) v2, (dReal(*)[4]) v3);
    //			  			      printf("Triangle %d: %2.1f %2.1f %2.1f, %2.1f %2.1f %2.1f, %2.1f %2.1f %2.1f\n", i,
    //						     v1[0], v1[1], v1[2], v2[0], v2[1], v2[2], v3[0], v3[1], v3[2]);
    glColor3f(1.0, 0.0, 0.0);
    glVertex3dv(v1);
    glColor3f(1.0, 0.0, 0.0);
    glVertex3dv(v2);
    glColor3f(1.0, 0.0, 0.0);
    glVertex3dv(v3);
  
  }
  glEnd();

   glPopMatrix();
  

}

void
Touchable::SetVel(double velx, double vely, double velz){
  mVelx = velx;
  mVely = vely;
  mVelz = velz;
  dBodySetLinearVel(mOdeBody, mVelx, mVely, mVelz);
  
}

Touchable::~Touchable(){
  free(tri);
  free(triVert);
}
